import matplotlib.pyplot as plt
from pylab import mpl
# 设置显示中文字体
mpl.rcParams["font.sans-serif"] = ["SimHei"]
# 设置正常显示符号
mpl.rcParams["axes.unicode_minus"] = False
class PID():
	def __init__(self,Kp, Ki, Kd , max, min, dt,intMax,intMin):
		self.dt = dt    # 循环时长
		self.max = max  # 操作变量最大值
		self.min = min  # 操作变量最小值
		self.Kp = Kp         # 比例增益
		self.Kd = Kd         # 积分增益
		self.Ki = Ki         # 微分增益
		self.integral = 0    # 直到上一次的误差值
		self.pre_error = 0   # 上一次的误差值
		self.iMax = intMax		#积分限幅
		self.iMin = intMin		#积分限幅
 
	def calculate(self, setPoint, pv):
                #setPoint 目标值
                #pv实际值
		# 其中 pv:process value 即过程值，
		error = setPoint - pv           # 计算得到偏差
		Pout = self.Kp * error          # 比例项
		if ((self.integral < self.iMax) and (self.integral>self.iMin)):
			self.integral += error * self.dt
		Iout = self.Ki * self.integral  # 积分项
		derivative = (error - self.pre_error)/self.dt
		Dout = self.Kd * derivative     # 微分项
 
		output = Pout + Iout + Dout     # 新的目标值
 
		if(output > self.max):
			output = self.max
		elif(output < self.min):
			output = self.min
 
		self.pre_error = error         # 保存本次误差，以供下次计算
		return output
 
t = range(100)
pidAngle = PID( 1, 0, 0.1, 2, -2,0.02,2,-2)
pidSpeed = PID( 1, 1, 0.001, 4.8, -4.8,0.02,12,-12)
AngleVal = 0 #初始实际值
SpeedVal2 = 0 #
Angle = []
AngleI = []
Speed = []
SpeedI = []
muBiaoZhi = 50
for i in t:
	AngleInc = pidAngle.calculate(muBiaoZhi, AngleVal)
	SpeedInc = pidSpeed.calculate(AngleInc,SpeedVal2)
	print("次数%d,实际角度:%f 本次误差:%f 积分值:%f---实际角速度:%f 本次误差:%f 积分值:%f"%(i,AngleVal,AngleInc,pidAngle.integral,SpeedVal2,SpeedInc,pidSpeed.integral))
	Angle.append(AngleVal)
	Speed.append(SpeedVal2)
	if (i ==50):
		muBiaoZhi = 0
	AngleI.append(pidAngle.integral)
	SpeedI.append(pidSpeed.integral)
	AngleVal  += AngleInc # 新的目标值累加作为模拟传感器采集的数值
	SpeedVal2 += SpeedInc # 新的目标值累加作为模拟传感器采集的数值
fig, ax = plt.subplots()    # 创建图实例
ax.plot(t, Angle, label='角度')    # 作图，并标记此线名
ax.plot(t, Speed, label='角速度')    # 作图，并标记此线名
ax.plot(t, AngleI, label='角度积分')    # 作图，并标记此线名
ax.plot(t, SpeedI, label='角速度积分')    # 作图，并标记此线名
ax.set_xlabel('PID调节次数')       #设置x轴名称
ax.set_ylabel('目标角度的误差')       #设置y轴名称
ax.set_title('#串级id')   #设置图名为
ax.legend() #自动检测要在图例中显示的元素，并且显示
plt.show()


